#include "Tips.h"


void searchEdgePoints( )
{
	//if ( binaryImage->nChannels != 1)
	//{
	//	cout<<"Error:Input for searchEdgePoints must be binary image."<<endl;
	//	exit(0);
	//}
	int height = binaryImage->height;
	int width = binaryImage->width;
	int widthStep = binaryImage->widthStep;

	memset(edgeHash, false, sizeof(bool)*height*width);

	uchar* ptr = (uchar*)(binaryImage->imageData);
	for (int i = 1; i < height-1; i++){
		for (int j = 1; j < width-1; j++){
			if ( ptr[i*widthStep+j] == 255 ) continue;
			if ( (ptr[(i-1)*widthStep+j] == 255) ||
				 (ptr[(i+1)*widthStep+j] == 255) ||
				 (ptr[ i*widthStep+j-1]  == 255) ||
				 (ptr[ i*widthStep+j+1]  == 255) ) 
			{
				edgeHash[i*width+j] = true;
			}
		}
	}

}

bool searchConnectivity(IplImage* binaryImage, 
	int rootx, int rooty, int* model,
	bool* tipHash_temp)
{
	if  (*(binaryImage->imageData+rootx*binaryImage->widthStep+rooty) != 0)
	{
		cout<<"Return without searching connectivity."<<endl;
		return false;
	}

	int pointSum = 0;
	queue<point*> listMargin;

	point* root = new point(rootx, rooty);
	listMargin.push(root);
	pointSum++;

	memset(model, 0, sizeof(int)*MODEL_HEIGHT*MODEL_WIDTH);
	//model[MODEL_HEIGHT/2][MODEL_WIDTH/2] = 1;
	model[(MODEL_HEIGHT/2)*MODEL_WIDTH + MODEL_WIDTH/2] = 1;
	uchar* ptr = (uchar*)(binaryImage->imageData);

	while (!listMargin.empty()){
		point* top = listMargin.front();
		listMargin.pop();

		int x = top->x;
		int y = top->y;

		int rx = x - rootx + MODEL_HEIGHT/2;
		int ry = y - rooty + MODEL_WIDTH/2;

		if ( (rx > 0) && (model[(rx-1)*MODEL_WIDTH+ry] == 0) &&
			(ptr[(x-1)*binaryImage->widthStep+y] == 0)){
				model[(rx-1)*MODEL_WIDTH + ry] = 1;
				listMargin.push( new point(x-1,y) );
				pointSum++;
		}
		if ( (rx < MODEL_HEIGHT-1) && (model[(rx+1)*MODEL_WIDTH+ry] == 0) &&
			(ptr[(x+1)*binaryImage->widthStep+y] == 0)){
				model[(rx+1)*MODEL_WIDTH + ry] = 1;
				listMargin.push( new point(x+1,y) );
				pointSum++;
		}
		if ( (ry > 0) && (model[rx*MODEL_WIDTH+ry -1] == 0 ) &&
			(ptr[x*binaryImage->widthStep+y-1] == 0) ){
				model[rx*MODEL_WIDTH + ry -1] = 1;
				listMargin.push( new point(x, y-1));
				pointSum++;
		}
		if ( (ry < MODEL_WIDTH-1) && (model[rx*MODEL_WIDTH+ry+1] == 0 ) &&
			(ptr[x*binaryImage->widthStep+y+1] == 0) ){
				model[rx*MODEL_WIDTH+ry+1] = 1;
				listMargin.push( new point(x, y+1));
				pointSum++;
		}

		delete top;
	}

	//If there are too many points,just return.
	if ((float)pointSum/(MODEL_HEIGHT*MODEL_WIDTH) > AREA_RATIO) 
	{
		return false;
	}
	//Find second area
	int rootX = 0;
	int rootY = 0;
	bool flag = false;
	for (int i = 0; i < MODEL_HEIGHT; i++){
		for (int j = 0; j < MODEL_WIDTH; j++){
			if ( model[i*MODEL_WIDTH+j] == 0 && !flag){
				model[i*MODEL_WIDTH+j] = 2;
				rootX = i + rootx - MODEL_HEIGHT/2;
				rootY = j + rooty - MODEL_WIDTH/2;
				flag = true;
			}
		}
	}


	listMargin.push(new point(rootX, rootY));
	pointSum++;

	while ( !listMargin.empty() ){
		point* top = listMargin.front();
		listMargin.pop();

		int x = top->x;
		int y = top->y;

		int rx = x - rootx + MODEL_HEIGHT/2;
		int ry = y - rooty + MODEL_WIDTH/2;

		if ( (rx > 0) && (model[(rx-1)*MODEL_WIDTH + ry] == 0) &&
			(ptr[(x-1)*binaryImage->widthStep+y] == 255)){
				model[(rx-1)*MODEL_WIDTH + ry] = 2;
				listMargin.push( new point(x-1,y) );
				pointSum++;
		}
		if ( (rx < MODEL_HEIGHT -1) && (model[(rx+1)*MODEL_WIDTH + ry] == 0) &&
			(ptr[(x+1)*binaryImage->widthStep+y] == 255)){
				model[(rx+1)*MODEL_WIDTH + ry] = 2;
				listMargin.push( new point(x+1,y) );
				pointSum++;
		}
		if ( (ry > 0) && (model[rx*MODEL_WIDTH + ry-1] == 0 ) &&
			(ptr[x*binaryImage->widthStep+y-1] == 255) ){
				model[rx*MODEL_WIDTH + ry-1] = 2;
				listMargin.push( new point(x, y-1));
				pointSum++;
		}
		if ( (ry < MODEL_WIDTH-1) && (model[rx*MODEL_WIDTH + ry+1] == 0 ) &&
			(ptr[x*binaryImage->widthStep+y+1] == 255) ){
				model[rx*MODEL_WIDTH + ry+1] = 2;
				listMargin.push( new point(x, y+1));
				pointSum++;
		}

		delete top;
	}
	if (pointSum != (MODEL_HEIGHT*MODEL_WIDTH)) return false;
	return true;
}

void mergeCloseTips(IplImage* binaryImage, int rootx, int rooty, bool* tipHash_temp)
{

	queue<point*> listMargin;

	point* root = new point(rootx, rooty);
	listMargin.push(root);

	//int model[MERGE_HEIGHT][MERGE_WIDTH];
	int* model = new int[MERGE_HEIGHT*MERGE_WIDTH];
	memset(model, 0, sizeof(int)*MERGE_HEIGHT*MERGE_WIDTH);

	//	model[MERGE_HEIGHT/2][MERGE_WIDTH/2] = 1;
	model[(MERGE_HEIGHT)/2*MERGE_WIDTH+MERGE_WIDTH/2] = 1;
	uchar* ptr = (uchar*)(binaryImage->imageData);

	while (!listMargin.empty()){
		point* top = listMargin.front();
		listMargin.pop();

		int x = top->x;
		int y = top->y;

		int rx = x - rootx + MERGE_HEIGHT/2;
		int ry = y - rooty + MERGE_WIDTH/2;

		if ( (rx > 0) && (model[(rx-1)*MERGE_WIDTH+ry] == 0) &&
			(ptr[(x-1)*binaryImage->widthStep+y] == 0)){
				model[(rx-1)*MERGE_WIDTH+ry] = 1;
				listMargin.push( new point(x-1,y) );
		}
		if ( (rx < MERGE_HEIGHT-1) && (model[(rx+1)*MERGE_WIDTH+ry] == 0) &&
			(ptr[(x+1)*binaryImage->widthStep+y] == 0)){
				model[(rx+1)*MERGE_WIDTH+ry] = 1;
				listMargin.push( new point(x+1,y) );
		}
		if ( (ry > 0) && (model[rx*MERGE_WIDTH+ry-1] == 0 ) &&
			(ptr[x*binaryImage->widthStep+y-1] == 0) ){
				model[rx*MERGE_WIDTH+ry-1] = 1;
				listMargin.push( new point(x, y-1));
		}
		if ( (ry < MERGE_WIDTH-1) && (model[rx*MERGE_WIDTH+ry+1] == 0 ) &&
			(ptr[x*binaryImage->widthStep+y+1] == 0) ){
				model[rx*MERGE_WIDTH+ry+1] = 1;
				listMargin.push( new point(x, y+1));
		}

		delete top;
	}
	for (int i = 0; i < MERGE_HEIGHT; i++){
		for (int j = 0; j < MERGE_WIDTH; j++){
			if ( model[i*MERGE_WIDTH+j] == 1 ){
				int rootX = i + rootx - MERGE_HEIGHT/2;
				int rootY = j + rooty - MERGE_WIDTH/2;
				tipHash_temp[rootX*binaryImage->width+rootY] = false;
			}
		}
	}
	tipHash_temp[rootx*binaryImage->width+rooty] = true;
}

void searchTipPoints()
{
	int* model = new int [MODEL_HEIGHT*MODEL_WIDTH];
	int height = binaryImage->height;
	int width = binaryImage->width;

	memcpy(tipHash, edgeHash, sizeof(bool)*height*width);

	for (int i = 0; i < height; i++){
		for (int j = 0; j < width; j++){
			if ( tipHash[i*width+j]&&
				searchConnectivity(binaryImage,i,j,model,tipHash))
			{ 
				tipList.push(new point(i,j));
				mergeCloseTips(binaryImage, i,j,tipHash);
			}
		}
	}	
	cout<<"Found "<<tipList.size()<<" tips."<<endl;
	uchar* ptr = (uchar*)binaryImage->imageData;

	memset(tipHash, false, sizeof(bool)*height*width);
	while ( !tipList.empty() ){
		point* top = tipList.front();
		tipList.pop();
		int x = top->x;
		int y = top->y;
		ptr[x*binaryImage->widthStep+y] = 120;
		tipHash[x*width+y]= true;
		delete top;
	}
	//saveImageTipsRaw(rawImage, binaryImage);

}

void saveImageTips(IplImage* binaryImage){
	int height = binaryImage->height;
	int width = binaryImage->width;
	IplImage* newImage = cvCreateImage(cvGetSize(binaryImage),IPL_DEPTH_8U,3);
	uchar* ptr = (uchar*)(newImage->imageData);
	memset(ptr, 0, sizeof(uchar)*height*width*3);
	uchar* rawPtr = (uchar*)binaryImage->imageData;
	for (int i = 0; i < height; i++){
		for (int j = 0; j < width; j++){
			ptr[i*newImage->widthStep+j*3]=rawPtr[i*width+j];
			ptr[i*newImage->widthStep+j*3+1]=rawPtr[i*width+j];
			ptr[i*newImage->widthStep+j*3+2]=rawPtr[i*width+j];
			if ( tipHash[i*width+j] == true ){
				ptr[i*newImage->widthStep+j*3+2]=255;
				ptr[i*newImage->widthStep+j*3+0]=0;
				ptr[i*newImage->widthStep+j*3+1]=0;
			}
		}
	}
	cvSaveImage("../Tips.bmp",newImage);
}

void saveImageTipsRaw(IplImage* rawImage, IplImage* binaryImage)
{
	int height = rawImage->height;
	int width = rawImage->width;
	int widthStep = rawImage->widthStep;
	IplImage* newImage = cvCreateImage(cvGetSize(rawImage), IPL_DEPTH_8U, 3);
	uchar* newPtr = (uchar*)newImage->imageData;
	uchar* rawPtr = (uchar*)rawImage->imageData;
	for (int i = 0; i < height; i++){
		for (int j = 0; j < width; j++){
			newPtr[i*widthStep+j*3] = rawPtr[i*widthStep+j*3];
			newPtr[i*widthStep+j*3+1] = rawPtr[i*widthStep+j*3+1];
			newPtr[i*widthStep+j*3+2] = rawPtr[i*widthStep+j*3+2];
			if ( *(binaryImage->imageData+i*width+j) == 120 ){
				newPtr[i*newImage->widthStep+j*3+2]=255;
				newPtr[i*newImage->widthStep+j*3+0]=0;
				newPtr[i*newImage->widthStep+j*3+1]=0;
			}
		}
	}
	cvSaveImage("../Tips.bmp",newImage);
}
